#include "icm206xx.h"

/********************静态变量定义********************/
const uint8_t icm_address=ICM20689_ADRESS;//imu地址
static imu_id_read icm_id=WHO_AM_I_NULL;//imu序列号
static int8_t icm_status=IMU_NOT_READY;//icm状态

/********************全局函数定义********************/

int8_t icm206xx_init()
{
  uint8_t icm_read_register[5]={0x00,0x02,0x08,0x08,0x03};//imu配置寄存器地址
  uint8_t icm_read_check[5]={0};//用于imu配置校验
	icm_status=i2c_write_one(IMU_I2C_INST,icm_address,PWR_MGMT_1, 0x81);//软件强制复位81
	delay_ms(100);
  icm_status=i2c_read_one(IMU_I2C_INST,icm_address,WHO_AM_I, &icm_id);

	switch(icm_id)
	{
		case WHO_AM_I_ICM20608D:
		case WHO_AM_I_ICM20608G:
		case WHO_AM_I_ICM20602:
		{
			icm_status=i2c_write_one(IMU_I2C_INST,icm_address,PWR_MGMT_1,0X80);	//复位ICM20608
			delay_ms(200);
			icm_status=i2c_write_one(IMU_I2C_INST,icm_address,PWR_MGMT_1, 0X01);	//唤醒ICM20608
			delay_ms(10);

      /*设置IMU*/
			icm_status=i2c_write_one(IMU_I2C_INST,icm_address,0x19, 0x00);   /* 输出速率是内部采样率 */
			icm_status=i2c_write_one(IMU_I2C_INST,icm_address,0x1A, 0x02);   /* 陀螺仪低通滤波BW=92Hz */
			icm_status=i2c_write_one(IMU_I2C_INST,icm_address,0x1B, 0x08);   /* 陀螺仪±500dps量程 */
			icm_status=i2c_write_one(IMU_I2C_INST,icm_address,0x1C, 0x08);   /* 加速度计±16G量程 */
			icm_status=i2c_write_one(IMU_I2C_INST,icm_address,0x1D, 0x03);   /* 加速度计低通滤波BW=448Hz */
			icm_status=i2c_write_one(IMU_I2C_INST,icm_address,0x6C, 0x00);   /* 打开加速度计和陀螺仪所有轴 */
			icm_status=i2c_write_one(IMU_I2C_INST,icm_address,0x1E, 0x00);   /* 关闭低功耗 */
			icm_status=i2c_write_one(IMU_I2C_INST,icm_address,0x23, 0x00);   /* 关闭FIFO */
			delay_ms(200);

      /*校验IMU设置*/
			for(uint8_t i=0x00;i<0x05;i++)
			{
        icm_status=i2c_read_one(IMU_I2C_INST,icm_address,0x19+i,&icm_read_check[i]);
        if(icm_status<IMU_OK)
        {
          break;
        }
				else if(icm_read_check[i]!=icm_read_register[i])
        {
          icm_status=IMU_INIT_ERROR;
          break;
        }
			}
		}
		break;
		default:
		{
      if(icm_status>=IMU_OK)icm_status=IMU_DO_NOT_SUPPORT;
		}
	}
	delay_ms(500);

  /*温漂校准(需要SPIFlash支持)*/
  // if(icm_status==IMU_OK)
  // {
  //   imu_calibration_params_init();
  // }
	return icm_status;
}

int8_t icm206xx_read_all(float* gyro_x,float* gyro_y,float* gyro_z,float* accel_x,float* accel_y,float* accel_z,float* temperature)
{
	uint8_t buf[14];
	int16_t temp;

  /*检查imu状态*/
  if(icm_status!=IMU_OK)return icm_status;
  
	icm_status=i2c_read(IMU_I2C_INST,icm_address,ACCEL_XOUT_H,buf,14);
  if(icm_status==IMU_OK)
  {
    switch(icm_id)
    {
      case WHO_AM_I_ICM20608D:
      case WHO_AM_I_ICM20608G:
      case WHO_AM_I_ICM20602:
      {
        *accel_x=-(int16_t)((buf[0]<<8)|buf[1]);
        *accel_y= (int16_t)((buf[2]<<8)|buf[3]);
        *accel_z=-(int16_t)((buf[4]<<8)|buf[5]);
        temp		=(int16_t)((buf[6]<<8)|buf[7]);
        *gyro_x	=-(int16_t)((buf[8]<<8)|buf[9]);
        *gyro_y	= (int16_t)((buf[10]<<8)|buf[11]);
        *gyro_z	=-(int16_t)((buf[12]<<8)|buf[13]);

        *temperature=25.0f+(double)((temp-25.0f)/326.8f);
      }
      break;
      default:
      {
        *accel_x=-(int16_t)((buf[0]<<8)|buf[1]);
        *accel_y=-(int16_t)((buf[2]<<8)|buf[3]);
        *accel_z= (int16_t)((buf[4]<<8)|buf[5]);
        temp		=(int16_t)((buf[6]<<8)|buf[7]);
        *gyro_x	=-(int16_t)((buf[8]<<8)|buf[9]);
        *gyro_y	=-(int16_t)((buf[10]<<8)|buf[11]);
        *gyro_z	= (int16_t)((buf[12]<<8)|buf[13]);
        *temperature=36.53f+(float)(temp/340.0f);
      }
    }

    /*单位化*/
	  *gyro_x*=GYRO_CALIBRATION_COFF_RAD;
	  *gyro_y*=GYRO_CALIBRATION_COFF_RAD;
	  *gyro_z*=GYRO_CALIBRATION_COFF_RAD;
    
    *accel_x*=ACC_CALIBRATION_COFF_M;
	  *accel_y*=ACC_CALIBRATION_COFF_M;
	  *accel_z*=ACC_CALIBRATION_COFF_M;
  }
  return icm_status;
}
